using System;
using System.Collections.Generic;
using System.Linq;
using Unity.Burst;
using Unity.Collections;
using Unity.Jobs;
using UnityEngine;
using UnityEngine.Experimental.AI;

namespace PathFinding
{
    public struct PNode : IEquatable<PNode>
    {
        public int x;
        public int z;
        public bool Walkable;
        public int GCost;
        public int HCost;
        public PathPolygonId polygonId;
        public PathPolygonId parent;

        public NativeList<PathPolygonId> Neighbors;

        public int FCost => GCost + HCost;
        public long Id => polygonId.Id;

        public bool Equals(PNode other)
        {
            return x == other.x && z == other.z && Walkable == other.Walkable && polygonId.Equals(other.polygonId);
        }

        public override bool Equals(object obj)
        {
            return obj is PNode other && Equals(other);
        }

        public override int GetHashCode()
        {
            return HashCode.Combine(x, z, Walkable, polygonId);
        }
    }
    public struct PathQuery 
    {
        //private AStarWorld mWorld;
        private NativeHashSet<PNode> openSet;
        private NativeHashSet<PNode> closedSet;
        private PNode endNode;
        public PathQuery(Allocator allocator, int iterations = 10)
        {
            //mWorld = world;
            openSet = new NativeHashSet<PNode>();
            closedSet = new NativeHashSet<PNode>();
        }

        public bool IsValid(PathPolygonId id)
        {
            return true;
        }

        [BurstDiscard]
        public int GetPathResult(NativeSlice<PathPolygonId> path)
        {
            var node = endNode;
            Stack<PathPolygonId> ss = new Stack<PathPolygonId>(path.Length);
            while (true)
            {
                var polygonId = node.polygonId;
                if (polygonId.IsNull())
                {
                    break;
                }
                ss.Push(polygonId);
                node = AStarWorld.GetDefaultWorld().GetNode(node.parent);
            }

            var i = 0;
            while (ss.Count > 0)
            {
                path[i++] = ss.Pop();
            }
            return i;
        }

        public PathPolyTypes GetPolygonType(PathPolygonId polygonId)
        {
            return PathPolyTypes.Ground;
        }

        public PathQueryStatus BeginFindPath(PathLocation from, PathLocation to, int areaMask = -1,
            NativeArray<float> costs = default(NativeArray<float>))
        {
            //if(MapLocation())
            var start = mWorld.GetNode(from.polygon);
            endNode = mWorld.GetNode(to.polygon);
            if (!start.walkable || !endNode.Walkable)
                return PathQueryStatus.InvalidParam;
            
            openSet.Add(start);
            return PathQueryStatus.InProgress;
        }

        public PathQueryStatus EndFindPath(out int polySize)
        {
            polySize = 0;
            return PathQueryStatus.Failure;
        }

        public PathQueryStatus UpdateFindPath(int iterations, out int iterationsPerformed)
        {
            iterationsPerformed = 0;
            while (openSet.Count > 0)
            {
                PNode? currentNode = null;
                //var currentP
                foreach (var node in openSet)
                {
                    if (currentNode == null || node.FCost < currentNode.Value.FCost ||
                        (node.FCost == currentNode.Value.FCost && node.HCost < currentNode.Value.FCost))
                    {
                        currentNode = node;
                    }
                }

                iterationsPerformed++;
                if (currentNode != null && currentNode.Value.Equals(endNode))
                {
                 //
                 return PathQueryStatus.Success;
                }
                if(currentNode == null)
                    throw new Exception("currentNode is null");
                openSet.Remove(currentNode.Value);
                closedSet.Add(currentNode.Value);

                var neighbours = AStarWorld.GetNeighbours(currentNode.Value.polygonId);
                foreach (var node in neighbours)
                {
                    var cp = node;
                    if (!node.Walkable || closedSet.Contains(node))
                    {
                        continue;
                    }

                    int gcost = node.GCost + 0;
                    if (gcost < node.GCost || !openSet.Contains(node))
                    {
                        cp.GCost = gcost;
                        cp.HCost = 0;
                        cp.parent = currentNode.Value.polygonId;

                        if (!openSet.Contains(node))
                        {
                            openSet.Add(cp);
                        }
                    }
                }

                if (--iterations == 0)
                {
                    return PathQueryStatus.InProgress;
                }
            }
            return PathQueryStatus.Failure;
        }

        public PathLocation MapLocation(Vector3 position, Vector3 extents, int typeId, int mask)
        {
            throw new System.NotImplementedException();
        }

        public void Dispose()
        {
            throw new NotImplementedException();
        }
    }

    public struct AStarWorld
    {
        private PNode[,] nodes;
        
        public static AStarWorld ActiveWorld { get; set; }

        public PathQuery CreatePathQuery(Allocator allocator)
        {
            return new PathQuery(this, allocator);
        }

        public void AddDependency(JobHandle handle)
        {
            
        }

        public PNode GetNode(PathPolygonId id)
        {
            var x = id.Id / 10000;
            var z = id.Id % 10000;
            return nodes[x, z];
        }

        public static NativeArray<PNode> GetNeighbours(PathPolygonId id)
        {
            return null;
        }
    }

    public struct PathPolygonId
    {
        public long Id { get; private set; }

        public PathPolygonId(long id = 0)
        {
            Id = id;
        }

        public bool IsNull()
        {
            return Id == 0;
        }

        public static bool operator ==(PathPolygonId lhs, PathPolygonId rhs)
        {
            return lhs.Id == rhs.Id;
        }

        public static bool operator !=(PathPolygonId lhs, PathPolygonId rhs)
        {
            return lhs.Id != rhs.Id;
        }
    }

    /// <summary>
    ///   <para>The types of nodes in the navigation data.</para>
    /// </summary>
    public enum PathPolyTypes
    {
        /// <summary>
        ///   <para>Type of node in the NavMesh representing one surface polygon.</para>
        /// </summary>
        Ground,

        /// <summary>
        ///   <para>Type of node in the NavMesh representing a point-to-point connection between two positions on the NavMesh surface.</para>
        /// </summary>
        OffMeshConnection,
    }

    /// <summary>
    ///   <para>Result information for AStar queries.</para>
    /// </summary>
    public struct PathHit
    {
        private Vector3 m_Position;
        private Vector3 m_Normal;
        private float m_Distance;
        private int m_Mask;
        private int m_Hit;

        /// <summary>
        ///   <para>Position of hit.</para>
        /// </summary>
        public Vector3 position
        {
            get => this.m_Position;
            set => this.m_Position = value;
        }

        /// <summary>
        ///   <para>Normal at the point of hit.</para>
        /// </summary>
        public Vector3 normal
        {
            get => this.m_Normal;
            set => this.m_Normal = value;
        }

        /// <summary>
        ///   <para>Distance to the point of hit.</para>
        /// </summary>
        public float distance
        {
            get => this.m_Distance;
            set => this.m_Distance = value;
        }

        /// <summary>
        ///   <para>Mask specifying NavMesh area at point of hit.</para>
        /// </summary>
        public int mask
        {
            get => this.m_Mask;
            set => this.m_Mask = value;
        }

        /// <summary>
        ///   <para>Flag set when hit.</para>
        /// </summary>
        public bool hit
        {
            get => this.m_Hit != 0;
            set => this.m_Hit = value ? 1 : 0;
        }
    }
}